home *** CD-ROM | disk | FTP | other *** search
/ Libris Britannia 4 / science library(b).zip / science library(b) / DDJMAG / DDJ9212.ZIP / algebra.c next >
C/C++ Source or Header  |  1992-09-17  |  10KB  |  283 lines
  1. /* ---------------------------------------------------------------- *
  2.  * ALGEBRA functions: A sampling of geometronical vector algebra    *
  3.  * functions and their supporting manifest constants and structure  *
  4.  * declarations.                                                    *
  5.  *                                                                  *
  6.  * The following code is derived from similar functions which are   *
  7.  * a small part of the Hipparchus Library. For simplicity, it lacks *
  8.  * the "fuzz control" and other programming elements of practical   *
  9.  * numerical significance.                                          *
  10.  *                                                                  *
  11.  * Programmer: Hrvoje Lukatela, September 1992.                     *
  12.  * Geodyssey Limited, Calgary - (403) 234 9848, fax: (403) 266 7117 *
  13.  ------------------------------------------------------------------ */
  14.  
  15. #include <math.h>
  16.  
  17. #define PI        3.14159265358979324
  18. #define DEG2RAD   (PI / 180.0)             /* degrees to radians... */
  19. #define RAD2DEG   (180.0 / PI)                /* ... and vice versa */
  20. #define LC_SCALE  32000.0          /* local coordinate scale factor */
  21.  
  22. struct plpt {              /* point in a Cartesian projection plane */
  23.    double est;
  24.    double nrt;
  25.    };
  26.  
  27. struct lclpt {                        /* local (object) coordinates */
  28.    short est;
  29.    short nrt;
  30.    };
  31.  
  32. struct dpxl {                         /* display screen coordinates */
  33.    short x;
  34.    short y;
  35.    };
  36.  
  37. struct ltln {                  /* point latitude-longitude, radians */
  38.    double lat;
  39.    double lng;
  40.    };
  41.  
  42. struct vct3 {                /* 3-D vector; x,y,z direction cosines */
  43.    double di;
  44.    double dj;
  45.    double dk;
  46.    };
  47.  
  48. struct vct2 {                   /* as above, in plane, internal use */
  49.    double di;
  50.    double dj;
  51.    };
  52.  
  53. struct indexRec {             /* line segment database index record */
  54.    struct vct3 center;               /* nominal object center point */
  55.    double      radius;                     /* in radian arc measure */
  56.    long        fileOffset;    /* offset in the coordinate data file */
  57.    short       vertexCount;         /* count of coordinate vertices */
  58.    short       segmentId;          /* for possible application use? */
  59.    };
  60.  
  61. /* ---------------------------------------------------------------- *
  62.  * Transform Latitude and Longitude Angles to Direction Cosines     *
  63.  * ---------------------------------------------------------------- */
  64.  
  65. void LatLongToDcos3(const struct ltln *pa, struct vct3 *pe) {
  66.  
  67.    double cosphi;
  68.  
  69.    cosphi = cos(pa->lat);
  70.    pe->di = cosphi * cos(pa->lng);
  71.    pe->dj = cosphi * sin(pa->lng);
  72.    pe->dk = sin(pa->lat);
  73.    return;
  74.    }
  75.  
  76. /* ---------------------------------------------------------------- *
  77.  * Transform Direction Cosines to Latitude and Longitude Angles     *
  78.  * ---------------------------------------------------------------- */
  79.  
  80. void Dcos3ToLatLong(const struct vct3 *pe, struct ltln *pa) {
  81.  
  82.    pa->lat = atan2(pe->dk, sqrt(pe->di * pe->di + pe->dj * pe->dj));
  83.    pa->lng = atan2(pe->dj, pe->di);
  84.    return;
  85.    }
  86.  
  87. /* ---------------------------------------------------------------- *
  88.  * Normalize a 3-D Direction Cosine Vector                          *
  89.  * ---------------------------------------------------------------- */
  90.  
  91. void NormalizeDcos3(struct vct3 *vcc) {
  92.  
  93.    double d;
  94.  
  95.    d = 1.0 / sqrt(vcc->di * vcc->di +
  96.                   vcc->dj * vcc->dj +
  97.                   vcc->dk * vcc->dk);
  98.    vcc->di *= d;
  99.    vcc->dj *= d;
  100.    vcc->dk *= d;
  101.    return;
  102.    }
  103.  
  104. /* ---------------------------------------------------------------- *
  105.  * Normalize a 2-D Direction Cosine Vector                          *
  106.  * ---------------------------------------------------------------- */
  107.  
  108. void NormalizeDcos2(struct vct2 *vcc) {
  109.  
  110.    double d;
  111.  
  112.    d = 1.0 / sqrt(vcc->di * vcc->di + vcc->dj * vcc->dj);
  113.    vcc->di *= d;
  114.    vcc->dj *= d;
  115.    return;
  116.    }
  117.  
  118. /* ---------------------------------------------------------------- *
  119.  * Spherical Arc (Great Circle Distance) - First Approximation      *
  120.  * ---------------------------------------------------------------- */
  121.  
  122. double ArcDist(const struct vct3 *pea, const struct vct3 *peb) {
  123.  
  124.    double chord, sqChord;
  125.  
  126.    sqChord = (peb->di - pea->di) * (peb->di - pea->di) +
  127.              (peb->dj - pea->dj) * (peb->dj - pea->dj) +
  128.              (peb->dk - pea->dk) * (peb->dk - pea->dk);
  129.    chord = sqrt(sqChord);
  130.    return(chord + ((sqChord * chord) / 24));
  131.    }
  132.  
  133. /* ---------------------------------------------------------------- *
  134.  * Direct Stereographic Projection (Map, Sphere to Plane)           *
  135.  * ---------------------------------------------------------------- */
  136.  
  137. void MapStereo(const struct vct3 *p0,
  138.                const struct vct3 *pe, struct plpt *pw) {
  139.    struct vct3 prln;
  140.    double      t, am, bm, ap, bp, cp, xi, yi, zi;
  141. /* ---------------------------------------------------------------- */
  142.  
  143. /* Find tangency point relative values.                             */
  144.  
  145.    cp = sqrt(p0->di * p0->di + p0->dj * p0->dj);
  146.    am = -(p0->dj / cp);
  147.    bm = p0->di / cp;
  148.    ap = -(p0->dk * bm);
  149.    bp = p0->dk * am;
  150.  
  151. /* Intersection of the projection line and the intersection plane.  */
  152.  
  153.    prln.di = -(p0->di + pe->di);
  154.    prln.dj = -(p0->dj + pe->dj);
  155.    prln.dk = -(p0->dk + pe->dk);
  156.  
  157.    NormalizeDcos3(&prln);
  158.  
  159.    t = -((p0->di * pe->di + p0->dj * pe->dj + p0->dk * pe->dk - 1.0) /
  160.          (p0->di * prln.di + p0->dj * prln.dj + p0->dk * prln.dk));
  161.  
  162.    xi = pe->di + prln.di * t;
  163.    yi = pe->dj + prln.dj * t;
  164.    zi = pe->dk + prln.dk * t;
  165.  
  166. /* Stereographic plane coordinates are the oriented distances from
  167.    the intersection point to the meridian and prime vertical plane. */
  168.  
  169.    pw->est = am * xi + bm * yi;
  170.    pw->nrt = ap * xi + bp * yi + cp * zi;
  171.    return;
  172.    }
  173.  
  174. /* ---------------------------------------------------------------- *
  175.  * Inverse Stereographic Projection (Un-Map, Plane to Sphere)       *
  176.  * ---------------------------------------------------------------- */
  177.  
  178. void UnMapStereo(const struct vct3 *p0,
  179.                  const struct plpt *pw, struct vct3 *pe) {
  180.    struct vct3   prln;
  181.    double        gcx, am, bm, ap, bp, cp, cpsq;
  182.    double        xe, ye, ze, xc, yc, zc, lymx, lxmy, root, t;
  183. /* ---------------------------------------------------------------- */
  184.  
  185. /* Find the sphere/plane tangency point values: ap, bp, cp are
  186.    components of the "North" vector, and am, bm of "East" vector
  187.    in this point. The "East" vector has no "Z" axis component.      */
  188.  
  189.    gcx = sqrt(pw->est * pw->est + pw->nrt * pw->nrt);
  190.  
  191.    cpsq = p0->di * p0->di + p0->dj * p0->dj;
  192.    cp = sqrt(cpsq);
  193.    am = -(p0->dj / cp);
  194.    bm = p0->di / cp;
  195.  
  196.    ap = -(p0->dk * bm);
  197.    bp = p0->dk * am;
  198.  
  199. /* Find Cartesian coordinates of the projection center (xc,yc,zc)
  200.    and the projected point in the plane of projection (xe,ye,ze).   */
  201.  
  202.    xc = -p0->di;
  203.    yc = -p0->dj;
  204.    zc = -p0->dk;
  205.  
  206.    xe = -xc + ap * pw->nrt + am * pw->est;
  207.    ye = -yc + bp * pw->nrt + bm * pw->est;
  208.    ze = -zc + cp * pw->nrt;
  209.  
  210. /* Find the intersection of ptc-pte line and the sphere.
  211.    Solution requires solving a quadratic in t, the line parameter.  */
  212.  
  213.    prln.di = -gcx;
  214.    prln.dj = -2.0;
  215.    NormalizeDcos2((struct vct2 *)&prln);
  216.    lymx = prln.dj * gcx - prln.di;
  217.    lxmy = -(prln.di * gcx + prln.dj);
  218.    root = sqrt(1.0 - (lymx * lymx));
  219.  
  220.    t = lxmy - root;  /* Find the closer of the two quadratic roots. */
  221.  
  222. /* Substitute the parameter in the parametric line equations.       */
  223.  
  224.    prln.di = xc - xe;
  225.    prln.dj = yc - ye;
  226.    prln.dk = zc - ze;
  227.    NormalizeDcos3(&prln);
  228.    pe->di = xe + t * prln.di;
  229.    pe->dj = ye + t * prln.dj;
  230.    pe->dk = ze + t * prln.dk;
  231.    NormalizeDcos3(pe);
  232.    return;
  233.    }
  234.  
  235. /* ---------------------------------------------------------------- *
  236.  * Initialize Projection Plane / Display Translation and Scaling    *
  237.  * ---------------------------------------------------------------- */
  238.  
  239. void SetPlaneDisplay(double *xfmArray,
  240.                      const struct plpt *w1, const struct plpt *w2,
  241.                      const struct dpxl *d1, const struct dpxl *d2) {
  242.  
  243.    double   dx, dy, du, dv;
  244.  
  245.    dx = (w2->est) - (w1->est);
  246.    dy = (w2->nrt) - (w1->nrt);
  247.    du = (double)((d2->x) - (d1->x));
  248.    dv = (double)((d2->y) - (d1->y));
  249.    xfmArray[0] = dx / du;
  250.    xfmArray[1] = dy / dv;
  251.    xfmArray[2] = du / dx;
  252.    xfmArray[3] = dv / dy;
  253.    xfmArray[4] = w1->est - xfmArray[0] * ((double)d1->x + 0.5);
  254.    xfmArray[5] = w1->nrt - xfmArray[1] * ((double)d1->y + 0.5);
  255.    xfmArray[6] = ((double)d1->x + 0.5) - xfmArray[2] * w1->est;
  256.    xfmArray[7] = ((double)d1->y + 0.5) - xfmArray[3] * w1->nrt;
  257.    return;
  258.    }
  259.  
  260. /* ---------------------------------------------------------------- *
  261.  * Translate/Scale Point from a Projection Plane to the Display     *
  262.  * ---------------------------------------------------------------- */
  263.  
  264. void PlaneToDisplay(const double *xfmArray,
  265.                     const struct plpt *w, struct dpxl *d) {
  266.  
  267.    d->x = (short)(xfmArray[6] + xfmArray[2] * w->est);
  268.    d->y = (short)(xfmArray[7] + xfmArray[3] * w->nrt);
  269.    return;
  270.    }
  271.  
  272. /* ---------------------------------------------------------------- *
  273.  * Translate/Scale Point from the Display to a Projection Plane     *
  274.  * ---------------------------------------------------------------- */
  275.  
  276. void DisplayToPlane(const double *xfmArray,
  277.                     const struct dpxl *d, struct plpt *w) {
  278.  
  279.    w->est = xfmArray[4] + xfmArray[0] * ((double)d->x + 0.5);
  280.    w->nrt = xfmArray[5] + xfmArray[1] * ((double)d->y + 0.5);
  281.    return;
  282.    }
  283.